/* 
 * Copyright (C) 2011 RobotCub Consortium, European Commission FP6 Project IST-004370
 * Author: Ugo Pattacini
 * email:  ugo.pattacini@iit.it
 * website: www.robotcub.org
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * http://www.robotcub.org/icub/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
*/

#include <stdio.h>

#include <yarp/os/Network.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/RateThread.h>
#include <yarp/os/Time.h>
#include <yarp/dev/Drivers.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/GazeControl.h>
#include <yarp/dev/CartesianControl.h>
#include <yarp/sig/Vector.h>
#include <yarp/math/Math.h>

#include <iCub/action/actionPrimitives.h>

YARP_DECLARE_DEVICES(icubmod)

using namespace std;
using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;
using namespace iCub::ctrl;
using namespace iCub::action;

class Controller : public RateThread
{
protected:
    string name;

    PolyDriver              drvGaze;
    IGazeControl           *iGaze;
    ICartesianControl      *iArm;
    ActionPrimitivesLayer2 *action;

    int gaze_contex_init;    

public:
    void configure(ResourceFinder &rf)
    {
        name=rf.check("name",Value("visualServoing")).asString().c_str();
    }

    bool threadInit()
    {
        Property optGaze("(device gazecontrollerclient)");
        optGaze.put("remote","/iKinGazeCtrl");
        optGaze.put("local",("/"+name+"/gaze").c_str());
        if (!drvGaze.open(optGaze))
            return false;

        drvGaze.view(iGaze);
        iGaze->storeContext(&gaze_context_init);

        return true;
    }

    void threadRelease()
    {
        iGaze->restoreContext(gaze_context_init);
        drvGaze.close();
    }

    void run()
    {
    }
};


class Launcher: public RFModule
{
protected:
    Controller controller;

public:
    bool configure(ResourceFinder &rf)
    {
        Time::turboBoost();

        controller.configure(rf);
        if (controller.start())
            return true;
        else
            return false;
    }

    bool close()
    {
        controller.stop();
        return true;
    }

    double getPeriod()
    {
        return 1.0;
    }

    bool updateModule()
    {
        return true;
    }
};


int main(int argc, char *argv[])
{
    Network yarp;
    if (!yarp.checkNetwork())
        return -1;

    YARP_REGISTER_DEVICES(icubmod)

    ResourceFinder rf;
    rf.setVerbose(true);
    rf.configure("ICUB_ROOT",argc,argv);

    Launcher launcher;    
    return launcher.runModule(rf);
}



